#include "antColonyOptimization.h"


void antColonyOptimization::G2D() {
    citys = vert.size();
    int k = vert.size() * vert.size();
    adjacencyMatrix = std::vector<std::vector<double>>(k, std::vector<double>(k, 0));

    for (int i = 0; i < k; i++) {
        for (int j = 0; j < k; j++) {
            if (judgeAdjacency(i, j))
                adjacencyMatrix[i][j] = 1;
        }
    }

    for (int i = 0; i < k; i++) {
        for (int j = 0; j < k; j++) {
            std::cout << adjacencyMatrix[i][j] << " ";
        }
        std::cout << std::endl;
    }
}

bool antColonyOptimization::judgeAdjacency(int p1, int p2) {
    if (p1 == p2)
        return false;

    int x1, x2, y1, y2;

    x1 = p1 / citys;  //向上取整
    x2 = p2 / citys;
    y1 = p1 % citys;
    y2 = p2 % citys;

    if (fabs(x1 - x2) <= 1 && fabs(y1 - y2) <= 1 && vert[x2][y2] == 0) {
        return true;
    }

    return false;
}

antColonyOptimization::antColonyOptimization(std::vector<std::vector<int>> vertPara) {
    vert = vertPara;
    G2D();
    //printf("input file_name and data type\n");
    //cin >> file_name >> type;
#ifdef _WIN32
    file_name = "salesman.in";
#else
    file_name = "../salesman.in";
#endif
    //type = 1;
    FILE* fp = fopen(file_name.c_str(), "r");
    char c[4];
    fgets(c, sizeof(int), fp);
    citys = (c[0] - '0');

    for (int i = 1; i < 4; i++) {
        if (c[i] >= '0' && c[i] <= '9')
            citys = citys * 10 + (c[i] - '0');
        else break;
    }
    //node = new vertex[N + 5];
    dis = std::vector<std::vector<double>>(citys, std::vector<double>(citys));
    double tmp = 0;
    int cnt = 0;

    for (int i = 0; i < citys; i++) {
        for (int j = 0; j < citys; j++) {
            fscanf(fp, "%lf", &dis[i][j]);
            tmp += i != j ? dis[i][j] : 0;// i == j的时候 dis不存在，所以不考虑。
            cnt += i != j ? 1 : 0;// i == j的时候dis不存在，所以不考虑。
        }
    }
    /*    for(int i=0;i<dis.size();i++){
            for(int j=0;j<dis[i].size();j++)
                cout <<dis[i][j] <<" ";
            cout << endl;
        }*/
        //cout << "tmp:" << tmp << endl;
        //cout << "cnt:" << cnt << endl;
    pheromone_0 = (double)cnt / (tmp * citys);//pheromone初始值，这里是1 / (avg * N)其中avg为图网中所有边边权的平均数。
    fclose(fp);
}

void antColonyOptimization::init() {
    rho = 0.3;//evaporation parameter，挥发参数，每次信息素要挥发的量 信息素挥发因子ρ
    alpha = 1;
    beta = 7;// alpha 和 beta分别表示pheromone(信息素)和heuristic的比重
    ants = citys;
    pheromone = std::vector<std::vector<double>>(citys, std::vector<double>(citys));
    heuristic = std::vector<std::vector<double>>(citys, std::vector<double>(citys));
    info = std::vector<std::vector<double>>(citys, std::vector<double>(citys));
    tau = std::vector<std::vector<double>>(citys, std::vector<double>(8, 0));
    r1 = std::vector<int>(citys);
    r = std::vector<int>(citys);
    s = std::vector<int>(citys);
    J = std::vector<std::set<int>>(citys);
    surplus.clear();
    for (int i = 0; i < citys; i ++){
        surplus.insert(i);
        for (int j = 0; j < citys; j ++){
            pheromone[i][j] = pheromone_0;
            heuristic[i][j] = 1 / (dis[i][j] + MINIMUM);//加一个小数minimum，防止被零除
        }
    }
    best_so_far.clean();
    iteration = 0;
    MAX = citys * citys;
}

void antColonyOptimization::reset() {
    tour = std::vector<Tour>(ants);
    for(int i = 0; i < citys; i++){
        tour[i].clean();
        r1[i] = i;
        J[i] = surplus;
        J[i].erase(r1[i]);//初始化agent i需要访问的城
        r[i] = r1[i];//当前在出发点
    }
    for (int i = 0; i < citys; i ++) {
        for (int j = 0; j < citys; j++) {
            info[i][j] = pow(pheromone[i][j], alpha) * pow(heuristic[i][j], beta);
        }//选择公式
    }
}

int antColonyOptimization::select_next(int k) {
    if (J[k].empty())
        return r1[k]; //如果J是空的，那么返回出发点城市
    double rnd = (double)(rand()) / (double)RAND_MAX;//产生0..1的随机数
    std::set<int>::iterator it = J[k].begin();
    double sum_prob = 0, sum = 0;
    for (; it != J[k].end(); it ++){
        sum += info[r[k]][*it];
    }//计算概率分布
    rnd *= sum;
    it = J[k].begin();
    for (; it != J[k].end(); it ++){
        sum_prob += info[r[k]][*it];
        if (sum_prob >= rnd){
            return *it;
        }
    }//依照概率选取下一步城市

    return -1;
}

void antColonyOptimization::construct_solution() {
    for (int i = 0; i < citys; i++){
        for (int k = 0; k < ants; k++) {
            int next = select_next(k);
            J[k].erase(next);
            s[k] = next;
            tour[k].push_back(r[k], s[k]);
            r[k] = s[k];
        }
    }
}

void antColonyOptimization::update_pheromone() {
    Tour now_best;
    now_best.clean();//初始化
    for (int i = 0; i < ants; i ++){
        tour[i].L = 0;
        int sz = tour[i].path.size();
        //计算长度
        for (int j = 0; j < sz; j ++){
            tour[i].L += dis[tour[i].path[j].first][tour[i].path[j].second];
        }
        if (tour[i] < now_best)
            now_best = tour[i];//寻找当前迭代最优解
    }

    if (now_best <best_so_far)
        best_so_far = now_best;//更新最优解

    for (int i = 0; i < citys; i++)
        for (int j = 0; j < citys; j++)
            pheromone[i][j] *= (1 - rho);//信息素挥发

    int sz = now_best.size();

    for (int i = 0; i < sz; i ++){
        pheromone[now_best.r(i)][now_best.s(i)] += 1.0 / (double)now_best.L;
        pheromone[now_best.s(i)][now_best.r(i)] = pheromone[now_best.r(i)][now_best.s(i)];//对称
    }//更新信息素含量
}

int antColonyOptimization::process() {
    init();
    double last = INT_MAX;
    int bad_times = 0;

    for(; iteration < MAX; iteration++){
        if (bad_times > citys)
            break;//进入局部最优
        reset();
        construct_solution();
        update_pheromone();
        std::cout << "iteration " << iteration << ":best_so_far = " << best_so_far.L << std::endl;
        if (last > best_so_far.L){
            last = best_so_far.L;
            bad_times = 0;
        } else bad_times++;
    }

    for(int i = 0; i < best_so_far.path.size(); i++){
        printf("(%d, %d) ", best_so_far.path[i].first, best_so_far.path[i].second);
    }
    printf("\n");

    std::cout << "best_so_far = " << best_so_far.L;

    return best_so_far.L;
}